package com.sparrow.common.ai.findpath.impl;

import com.sparrow.common.ai.findpath.PathNode;
import com.sparrow.common.object.ObjectPool;
import java.util.*;

/**
 * 在进行启发式搜索提高算法效率的同时，可以保证找到一条最优路径
 * A*算法的估算函数  f(n) = g(n) + h(n)
 * g(n) 表示从起点到任意点n的实际距离
 * h(n) 从点n到目标点的估算距离
 * h(n) 又称为评估函数,常见的评估函数有 欧几里得距离、曼哈顿距离、切比雪夫距离,这里使用切比雪夫距离
 *
 * Dijkstra 算法计算距起点的距离。贪心最佳优先搜索估计到目标点的距离。A* 正在使用这两个距离的总和。
 *
 * 地图数据 map[y][x] y=行 x=列
 *
 * AStar 优化：
 * 1 走斜线的情况下，如果正交两个方向不可走，那么斜线也不可走，避免穿墙
 * 2 close set 用二维数组 代替 HashSet 节省内存和提高性能
 * 3 搜索获取周围邻居节点时 使用缓存的列表 避免每次创建列表
 * 4 用额外的OpenSetMap 提升OpenSet的查找效率,避免O(n)查找
 * 5 获取PathNode时 使用对象池，降低创建和销毁对象带来的开销同时减轻GC压力
 */
public class AStar {
    /**
     * 将要被估算的节点集合，初始只包含起始点
     */
    private PriorityQueue<PathNode> openSet = new PriorityQueue<>();
    /**
     * 用于提高优先级队列性能的 map
     */
    private Map<PathNode, PathNode> openSetMap = new HashMap<>();
    /**
     * 已经被估算的节点集合
     */
    private boolean[][] closed;
    /**
     * 缓存邻居列表 避免每次创建
     */
    private final List<PathNode> neighbors = new ArrayList<>(8);
    /**
     * 对象池复用 避免频繁创建对象和 gc压力
     */
    private ObjectPool<PathNode> objectPool = new ObjectPool<>(PathNode::new, 10, 10000, false);

    public List<int[]> findPath(byte[][] mapData, int[] start, int[] goal) {
        if (closed == null || closed.length != mapData.length || closed[0].length != mapData[0].length) {
            closed = new boolean[mapData.length][mapData[0].length];
        } else {
            // 重置
            for (int y = 0; y < closed.length; y++) {
                Arrays.fill(closed[y], false);
            }
        }
        PathNode startNode = objectPool.get();
        startNode.setX(start[0]);
        startNode.setY(start[1]);
        addOpenSet(startNode);

        PathNode goalNode = objectPool.get();
        goalNode.setX(goal[0]);
        goalNode.setY(goal[1]);
        if (!canMove(mapData, goalNode) || !canMove(mapData, startNode)) {
            recycleNode(startNode);
            recycleNode(goalNode);
            return Collections.emptyList();
        }

        PathNode findNode = null;
        while (!openSet.isEmpty()) {
            PathNode current = openSet.remove();
            openSetMap.remove(current);
            if (current.equals(goalNode)) {
                findNode = current;
                break;
            }
            addCloseSet(current);
            for (PathNode next : getNeighbors(mapData, current)) {
                if (isInCloseSet(next) || !canMove(mapData, next)) {
                    recycleNode(next);
                    continue;
                }
                //从起点到next点的实际距离
                int g = current.getG() + g(current, next);
                //从点next到目标点的估算距离
                int h = h(next, goalNode);
                //启发函数
                int f = g + h;
                if (!openSetMap.containsKey(next)) {
                     next.setG(g);
                     next.setF(f);
                     next.setParent(current);
                     addOpenSet(next);
                } else { //已经在openSet
                    PathNode exist = getNodeFromOpenSet(next);
                    if (f < exist.getF()) { //更短的路径
                        removeOpenSet(exist);
                        next.setG(g);
                        next.setF(f);
                        next.setParent(current);
                        addOpenSet(next);
                    } else {
                        recycleNode(next);
                    }
                }
            }
        }

        List<int[]> path = buildPath(findNode);
        clear();
//        recycleNode(startNode);
        recycleNode(goalNode);
        return path;
    }

    private boolean isInCloseSet(PathNode node) {
        return closed[node.getY()][node.getX()];
    }

    private void addCloseSet(PathNode node) {
        closed[node.getY()][node.getX()] = true;
    }

    private void removeOpenSet(PathNode exist) {
        openSet.remove(exist);
        openSetMap.remove(exist);
    }

    private void addOpenSet(PathNode node) {
        openSet.add(node);
        openSetMap.put(node, node);
    }

    private void clear() {
        // 清理 openSet 中剩余节点
        for (PathNode n : openSet) {
            objectPool.recycle(n);
        }
        openSet.clear();
        openSetMap.clear();
        // 重置
        for (int y = 0; y < closed.length; y++) {
            Arrays.fill(closed[y], false);
        }
    }

    public boolean canMove(byte[][] mapData, PathNode n) {
        return canMove(mapData, n.getX(), n.getY()); // ✅ y 行, x 列
    }

    public boolean canMove(byte[][] mapData, int x, int y) {
        return mapData[y][x] == 0;
    }

    private PathNode getNodeFromOpenSet(PathNode n) {
        return openSetMap.get(n);
    }

    private int h(PathNode a, PathNode b) {
        return h2(a, b);
    }

    private int h0(PathNode a, PathNode b) {
        int dx = Math.abs(a.getX() - b.getX());
        int dy = Math.abs(a.getY() - b.getY());
        return 10 * (dx + dy) - 6 * Math.min(dx, dy); // 斜线 14，直线 10
    }

    /**
     * 曼哈顿距离, 适用于只能上下左右移动的方格地图
     * @param a
     * @param b
     * @return
     */
    private int h1(PathNode a, PathNode b) {
        //方形网格的曼哈顿距离
        int dx = Math.abs(a.getX() - b.getX());
        int dy = Math.abs(a.getY() - b.getY());
        return 10 * (dx + dy); // 曼哈顿距离统一倍率
    }

    /**
     * 切比雪夫距离
     * @param a
     * @param b
     * @return
     */
    private int h2(PathNode a, PathNode b) {
        int dx = Math.abs(a.getX() - b.getX());
        int dy = Math.abs(a.getY() - b.getY());
        return 10 * (dx + dy) + (14 - 20) * Math.min(dx, dy);
    }

    /**
     * 欧几里得距离
     * @param a
     * @param b
     * @return
     */
    private int h3(PathNode a, PathNode b) {
        int dx = a.getX() - b.getX();
        int dy = a.getY() - b.getY();
        // 用平方代替 开平方根
        //    return (int) (10 * Math.sqrt(dx * dx + dy * dy));
        return dx * dx + dy * dy; // 省略开方，f值比较时仍然有效
    }

    /**
     * 当前点和下个点的移动代价 斜边代价更高
     * 根据勾股定理计算出来的近似值 c平方 = a平方 + b平方 c 约等于 14.14 取整的话 14左右
     *
     * @param current
     * @param next
     * @return
     */
    private int g(PathNode current, PathNode next) {
        int dx = Math.abs(next.getX() - current.getX());
        int dy = Math.abs(next.getY() - current.getY());
        //斜边的话 dx == 1 && dy == 1
        return 14 * Math.min(dx, dy) + 10 * Math.abs(dx - dy);
    }

    /**
     * 获得点n 周围8个邻居
     * @param mapData
     * @param n
     * @return
     */
    public List<PathNode> getNeighbors(byte[][] mapData, PathNode n) {
        neighbors.clear();

        for (int dy = -1; dy <= 1; dy++) {
            int y = n.getY() + dy;
            for (int dx = -1; dx <= 1; dx++) {
                int x = n.getX() + dx;
                if (x < 0 || y < 0 || y >= mapData.length || x >= mapData[0].length) {
                    continue;
                }
                if (x == n.getX() && y == n.getY()) {
                    continue;
                }
                // ✅ 斜角剪枝, 避免穿墙
                if (dx != 0 && dy != 0) {
                    // 任意正交方向不可走，则禁止斜角
                    if (!canMove(mapData, n.getX() + dx, n.getY()) ||
                            !canMove(mapData, n.getX(), n.getY() + dy)) {
                        continue;
                    }
                }
                PathNode next = objectPool.get();
                next.setX(x);
                next.setY(y);
                neighbors.add(next);
            }
        }
        return neighbors;
    }

    /** 回收节点，避免重复代码 */
    private void recycleNode(PathNode node) {
        if (node != null) {
            objectPool.recycle(node);
        }
    }

    /** 从找到的节点回溯路径，同时顺手回收链路节点 */
    private List<int[]> buildPath(PathNode findNode) {
        if (findNode == null) {
            return Collections.emptyList();
        }
        LinkedList<int[]> path = new LinkedList<>();
        PathNode node = findNode;
        while (node != null) {
            path.addFirst(new int[]{node.getX(), node.getY()});
            PathNode parent = node.getParent();
            recycleNode(node); // ✅ 顺手回收
            node = parent;
        }
        return path;
    }


    public static void printPaths(List<int[]> paths) {
        StringBuilder sb = new StringBuilder();
        for (int i = 0; i < paths.size(); i++) {
            int[] n = paths.get(i);
            if (i == paths.size() - 1) {
                sb.append("(").append(n[0]).append(",").append(n[1]).append(")");
            } else {
                sb.append("(").append(n[0]).append(",").append(n[1]).append(")->");
            }
        }
        System.out.println(sb);
    }


}
